#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <string>
#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>

int main(int argc, char* argv[]) {
    cv::Size boardSize = cv::Size(11, 8);  //棋盘格大小
    cv::Size imageSize = cv::Size(1624, 1240);  //图片大小
    std::vector<std::string> imageListLeft, imageListRight;
    std::string n_str;
    //float squareSize = 1.5f; // 单位：mm

    // 真实世界物理坐标
    std::vector<cv::Point3f> obj;
    for (int y=0; y<boardSize.height; y++) {
        for (int x=0; x<boardSize.width; x++) {
            obj.push_back(cv::Point3f(x*10, y*10, 0.0f));  // 间隔10mm
        }
    }

    for (int i=1; i<=22; i++) {
        n_str = std::to_string(i);
        imageListLeft.push_back("/home/dell3660/vscodeprojects/calibration/stereo_calibrate_image/l" + n_str + ".bmp");
        imageListRight.push_back("/home/dell3660/vscodeprojects/calibration/stereo_calibrate_image/r" + n_str + ".bmp");
    }

    // 用作标定的图片数量
    int image_num;

    if (imageListLeft.size()==imageListRight.size()) {
        image_num = imageListLeft.size();
        std::cout << "双目标定所用到的图片数目为：" << image_num*2 << std::endl;
    }
    else{
        std::cout << "左右视图图像数目不一致！" << std::endl;
        return -1;
    }


    std::vector<cv::Point2f> cornersLeft, cornersRight;
    std::vector<int> cornerCounts;
    // float squareSize = 1.5;

    std::vector<std::vector<cv::Point3f>> objectPoints;
    std::vector<std::vector<cv::Point2f>> imagePointsLeft, imagePointsRight;

    // std::string findCornerPath = "/home/ma/StereoCalibration/findCorners/";

    for (int i=0; i<image_num; i++) {
        cv::Mat imageLeft = cv::imread(imageListLeft[i], cv::IMREAD_GRAYSCALE);
        cv::Mat imageRight = cv::imread(imageListRight[i], cv::IMREAD_GRAYSCALE);

        bool foundLeft = cv::findChessboardCorners(imageLeft, boardSize, cornersLeft, cv::CALIB_CB_ADAPTIVE_THRESH+cv::CALIB_CB_NORMALIZE_IMAGE);
        bool foundRight = cv::findChessboardCorners(imageRight, boardSize, cornersRight, cv::CALIB_CB_ADAPTIVE_THRESH+cv::CALIB_CB_NORMALIZE_IMAGE);

        if (foundLeft && foundRight) {
            std::cout << i << std::endl;
            // 可视化角点
            // cv::drawChessboardCorners(imageLeft, boardSize, cornersLeft, foundLeft);
            // cv::drawChessboardCorners(imageRight, boardSize, cornersRight, foundRight);
            // cv::imwrite(findCornerPath+std::to_string(i)+"_L.bmp", imageLeft);
            // cv::imwrite(findCornerPath+std::to_string(i)+"_R.bmp", imageRight);

            cv::cornerSubPix(imageLeft, cornersLeft, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.001));
            cv::cornerSubPix(imageRight, cornersRight, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.001));

            if (cornersLeft.size() == boardSize.area() && cornersRight.size() == boardSize.area()) {
                objectPoints.push_back(obj);
                imagePointsLeft.push_back(cornersLeft);
                imagePointsRight.push_back(cornersRight);
            }
        }
    }

    // 左右相机参数
    cv::Mat cameraMatrixLeft = cv::Mat::zeros(3, 3, CV_64F);
    cv::Mat distCoeffsLeft = cv::Mat::zeros(5, 1, CV_64F);
    cv::Mat RLeft = cv::Mat::zeros(3, 3, CV_64F);
    cv::Mat TLeft = cv::Mat::zeros(3, 1, CV_64F);
    cv::calibrateCamera(objectPoints, imagePointsLeft, imageSize, cameraMatrixLeft, distCoeffsLeft, RLeft, TLeft);

    cv::Mat cameraMatrixRight = cv::Mat::zeros(3, 3, CV_64F);
    cv::Mat distCoeffsRight = cv::Mat::zeros(5, 1, CV_64F);
    cv::Mat RRight = cv::Mat::zeros(3, 3, CV_64F);
    cv::Mat TRight = cv::Mat::zeros(3, 1, CV_64F);
    cv::calibrateCamera(objectPoints, imagePointsRight, imageSize, cameraMatrixRight, distCoeffsRight, RRight, TRight);
  
    cv::Mat R, T, E, F;
    double RMS = cv::stereoCalibrate(objectPoints, imagePointsLeft, imagePointsRight,
                                     cameraMatrixLeft, distCoeffsLeft,
                                     cameraMatrixRight, distCoeffsRight,
                                     cv::Size(2448, 2048), R, T, E, F,
                                    cv::CALIB_FIX_INTRINSIC, cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 100, 1e-5));
    // 输出结果  
    std::cout << "RMS: " << RMS << std::endl;
    std::cout << "Rotation Matrix:\n" << R << std::endl;
    std::cout << "Translation Vector:\n" << T << std::endl;

}